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Rapid,  Robust,  Optimal  Pose  Estimation  from  a  Single  Affine  Image 

John  E.  Mclnroy,  Senior  Member,  IEEE,  R.  Scott  Erwin  and  Lawrence  M.  Robertson 


Abstract — Determining  the  rigid  transformation  relating  a  2d 
image  to  known  geometry  is  a  classical  problem  in  computer 
vision.  To  date,  the  most  accurate  methods  require  performing 
an  unknown  number  of  iterations  until  a  numerical  algorithm 
converges  to  the  desired  tolerance.  For  the  case  of  affine  imaging, 
this  paper  replaces  these  nonlinear  numerical  iterations  with 
solving  the  standard  3d-3d  optimal  orientation  problem  2n 
times,  where  n  is  the  number  of  data  points.  The  2n  successive 
optimal  orientation  calculations  are  speeded  through  use  of  Gray 
code,  and  have  the  dual  advantages  of  speed  and  predictable 
execution  time.  Angular  errors  caused  by  scaling  imperfections 
are  quantified,  and  a  least  upper  bound  estimate  of  the  scaling 
is  proposed.  It  is  shown  that  the  worst  case  viewpoints  depend 
only  on  the  data  points  chosen,  and  a  new  convex  linear  matrix 
inequality  optimization  is  derived  for  determining  the  worst 
viewpoint.  This  new  analysis  tool  is  useful  for  evaluating  a 
particular  set  of  data  and  suggests  methods  of  designing  the 
data  for  high  performance. 

Index  Terms —  affine  cameras,  pose  estimation,  visual  servoing, 
template  matching,  object  recognition 

I.  Introduction 

Determining  the  rigid  transformation  relating  an  image  to 
known  geometry,  i.e.  pose  estimation,  is  a  central  problem  in 
computer  graphics,  computer  vision,  robotics,  and  photogram- 
metry.  In  computer  graphics,  it  is  important  in  tasks  which 
combine  computer  generated  objects  with  natural  photos.  In 
computer  vision,  it  is  central  to  object  recognition.  In  robotics, 
it  is  useful  for  coordinating  the  motion  of  the  hand  and  the 
eye.  In  photogrammetry,  it  is  key  to  detailed  inspection  from 
grainy  images. 

Iterative,  nonlinear  numerical  optimizations  can  provide 
fully  optimal  solutions  which  are  therefore  of  the  highest 
possible  accuracy.  There  are  many  examples  of  such  methods. 
Since  this  paper  is  concerned  with  rapid  techniques,  [10]  is 
a  fast  iterative  technique  based  on  object,  rather  than  image, 
iterations.  Global  convergence  is  found.  Both  [3]  and  [5]  use 
iteratively  re-weighted  least  squares  techniques  for  tracking 
rapidly.  These  approaches  are  based  on  linearization  using  the 
Lie  group’s  infinitestimal  generator,  thus  they  are  suitable  for 
tracking  small  changes  in  pose  between  images.  Let  SO(3) 
denote  the  three  dimensional  Special  Orthogonal  group  which 
models  rotation.  Cyclic  coordinate  descent,  which  alternately 
finds  the  optimal  rotation,  R  £  SO( 3),  then  Euclidean  terms 
such  as  the  translation,  then  recalculates  R  and  so  forth  is  used 
for  camera  calibration  [12],  [8]. 

This  work  was  supported  by  the  National  Research  Council  and  Air  Force 
Office  of  Scientific  Research. 

J.  Mclnroy  is  with  the  Department  of  Electrical  and  Computer  Engineering 
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87117-5776,  E-mail :  lawrence  .  robert son@ kirtland .  af  . mil , 
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Closed  form  solutions  are  available  for  n  =  3  or  n  =  4 
correspondence  points.  The  roots  of  a  fourth  or  fifth  order 
polynomial  contain  the  solution  ([4]  provides  one  example 
algorithm  and  references  for  other  algorithms).  For  a  small 
set  of  points  when  n  >  4,  non-iterative  techniques  methods 
are  found  in  [14],  [6],  and  [1],  The  solution  in  [1]  solves 
a  quadratic  problem  through  an  over-parameterization,  then 
multiple  linear  singular  value  decompositions.  The  number  of 
variables  generated  can  be  high.  For  instance,  the  constraint 
RT R  =  I  requires  45  variables  to  represent  this  three 
dimensional  element  in  SO(3). 

Despite  the  availability  of  many  algorithms,  there  remains 
a  need  for  a  rapid,  optimal,  dependable  algorithm  suitable 
for  a  small  number  of  point  correspondences  (but  more  than 
n  =  4).  For  instance,  [15]  recently  added  high  bandwidth 
inertial  sensing  to  complement  the  available  pose  estimation 
because  it  alone  had  too  low  of  a  bandwidth.  For  the  case 
of  affine  imaging,  this  paper  develops  a  new  method  that  is 
closed  form,  yet  provides  optimal  estimates  even  when  n  >  4. 
It  does  so  by  solving  the  standard  3d-3d  optimal  orientation 
problem  2n  times,  where  n  is  the  number  of  data  points. 
Since  the  optimal  orientation  problem  can  be  very  quickly 
solved  as  a  4  x  4  matrix  eigenvalue  problem,  the  overall 
computational  burden  is  light.  However,  the  method  clearly 
becomes  less  attractive  for  large  n,  as  2"  becomes  extremely 
large.  To  help  offset  this  problem,  the  2™  successive  optimal 
orientation  calculations  are  speeded  through  use  of  Gray  code, 
thus  they  have  the  dual  advantages  of  speed  and  predictable 
execution  time.  Angular  errors  caused  by  scaling  imperfections 
are  quantified,  and  a  least  upper  bound  estimate  of  the  scaling 
is  proposed.  It  is  shown  that  the  worst  case  viewpoints  depend 
only  on  the  data  points  chosen,  and  a  new  convex  linear  matrix 
inequality  optimization  is  derived  for  determining  the  worst 
viewpoint.  This  new  analysis  tool  is  useful  for  evaluating 
a  particular  set  of  data.  In  addition,  it  suggests  methods  of 
designing  the  data  for  high  performance. 

II.  Optimal  Estimation  of  Pose  from  3-D  Data 

First,  classical  techniques  [17]  for  solving  the  optimal 
orientation  problem  will  be  briefly  reviewed.  Quaternion  based 
methods  will  be  used  here-see  [10]  for  a  description  of  sin¬ 
gular  value  decomposition  based  methods.  A  complete  proof 
of  the  optimal  quaternion  algorithm  is  presented  to  emphasize 
the  structure  of  the  solution.  This  bilinear  structure  will  be 
heavily  exploited  later  in  this  paper. 

From  an  object’s  geometry,  direction  vectors  2]  €  K3,  i  = 
1  are  known  in  an  object’s  coordinate  frame.  Those 

same  vectors,  £  R3,  are  measured  in  a  sensor  coordinate 
frame.  Theorem  1  provides  optimal  estimates  of  the  rotation 
matrix  from  the  object  to  the  sensor  frame,  R.  First,  define 
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the  hat  function  (•)  as  the  cross  product  matrix,  i.e. 

0  -z3  z2 

z=  z3  0  —zi 
—z2  z  1  0 

Theorem  1:  Given  Zi,  Vi  £  K3  and  w,  £  R+,  i  =  1 ...  n, 

the  minimum  of 


j  =  wj\\Rzi  - 1 


over  R  £  SO(3),  a  £  R+  is  found  by  first  calculating 


D  = 


These  can  be  combined  into  a  single  vector  derivative 


d'lTRz 


—‘lvTzI  +  ZV1"  +  VZ1,  zv 


*=  2  K(v,z)i 


Note  that  the  matrix  function  K (v,  z)  is  symmetric  and  linear 
in  each  of  its  arguments  (bilinear  overall). 

Applying  these  formulas  to  each  term  of  the  Lagrangian, 
the  necessary  conditions  are  found  to  be 


=  0  =  —2a  ^  Wi  [— 2vf 


zi  +  zivi  +vizi\e 


—2a  Wizve  4  —  2Ae 


where  K  is  the  bilinear,  symmetric  matrix  function 


K  (v,  z)  — 


vz T  +  zif  —  2vrzl  zv 


The  eigenvector,  e,  of  D  corresponding  to  the  maximum 
eigenvalue  is  the  unit  quaternion  representation  of  the  optimal 
R.  The  optimal  scaling  is 

(4) 

ElU^INI2 

Proot:  Any  R  £  SO(3)  can  be  written  as  the  following 
quadratic  function  of  a  unit  quaternion: 

R  =  2[(0.5-ere)I+e&r +  e4e\  (5) 

where  e5"  =  [ei  e2  e2  e 4 ]  £  R4  is  the  unit  quaternion.  The 
first  three  elements  of  e  form  the  e  vector,  =  [ei  e2  €3]. 
The  optimization  problem  with  objective  (1)  can  be  rewritten 
in  terms  of  unit  quaternions  as 

n 

minV^  m;||2[(0.5  —  (F  e)I  +  eeF  +  e^Zi  —  anj||2  (6) 

i= 1 

subject  to  e^e  =  1.  Multiplying  out  this  equation,  and 
discarding  terms  that  are  not  a  function  of  either  e  or  a,  the 
Lagrangian  can  be  formed: 

n 

L  =  —2a  Wi  vj  2[(0.5  —  eT  e)  I  +  eeT  +  e^Zi 


+«!E' 


+  A(1  -  eTe) 


where  A  is  a  Lagrange  multiplier.  Before  calculating  the 
necessary  conditions  for  an  optimum,  first  consider  the  simpler 
term  vr Rz  =  2[0.5vTz  —  vTerez  +  vreerz  +  e^vrez\.  Since 
xTy  =  iTx,  xy  =  —yx  and  x  =  —a?  for  any  vectors  x  and 
y,  v1-  Rz  =  2 [0.5 vTz  —  vrzeTe  +  erzvre  —  €4 vTze\.  Then 


cR-f'R.z 


=  2  [—2  vT  ze  +  z'iFe  +  vzre  +  €4  zv\ 


dv TRz 


=  2[zv\Te 


=  0  =  —2a  Wi2[zv\Te  —  2Ae4 


oL/  ^ 

—  =  0  =  —4a  ^2  wiK(vi,  Zi)e-  2Xe  (9) 

i=l 

The  matrix  D  =  5Z”=i  Rj  is  a  3  x  3  symmetric 

matrix  that  can  be  calculated  directly  from  the  given  data. 
The  optimal  quaternion  is  the  solution  to 

—2aDe  =  Xe 

Thus  e  is  an  eigenvector  of  D.  Note  that  the  partial  of  the 
objective  is 

g  j 

—  =  -4  aDe  =  2Xe 
ae 

Thus  J  =  2Xer e  =  2X  plus  terms  constant  in  e.  The  minimal 
value  of  J  is  thus  given  by  the  minimal  eigenvalue  A  of  —2 aD. 
Since  a  scales  all  the  eigenvalues  equally,  the  minimum  J  is 
given  by  the  maximum  eigenvalue  of  D.  Hence  the  optimal 
quaternion  is  that  eigenvector  of  D  corresponding  to  the 
maximum  eigenvalue.  Once  e  is  found,  the  optimal  rotation 
matrix  can  be  found  from  (5). 

Note  that  the  optimal  R  is  independent  of  the  scaling.  Thus 
scaling  all  sensor  measurements,  v, ,  by  the  same  amount  will 
not  affect  the  orientation  estimate.  Once  R  is  found,  it  can  be 
used  to  calculate  the  scaling  term.  The  necessary  condition  for 
optimality  is 

dL  dJ  n  n  A  m^m2 

—  =  —  =  °  =  -2^  WiVi  Rzi  +  2 a  2^  wx  ||Vi  1 1 

t— I  i- 1 

Rearranging  gives  (4). 


Remarks:  Theorem  1  is  a  classical  result  [17].  It  requires 
solution  of  a  4  x  4  matrix  eigenvalue  problem.  This  paper  uti¬ 
lizes  it  because  it  is  a  standard  technique,  but  it  should  be  noted 
that  other  faster  techniques  may  be  fruitful  for  integration 
with  the  this  paper’s  main  new  contributions.  For  instance,  [9] 
suggests  an  optimal  orientation  algorithm  requiring  solution 
of  only  a  3  x  3  eigenvalue  problem. 

As  Theorem  1  shows,  the  solution  is  invariant  with  respect 
to  scaling.  This  effect  will  be  of  crucial  importance  when  this 


IEEE  TRANSACTIONS  ON  ROBOTICS,  VOL.  X,  NO.  XX,  NOVEMBER  200X 


3 


algorithm  is  extended  further  to  use  2-dimensional  (2-d),  rather 
than  3 -dimensional  (3-d)  data.  In  addition,  this  formulation  of 
the  solution  highlights  a  powerful  property  (bilinearity  of  K) 
that  will  be  exploited  for  pose  estimation  from  both  3-d  and  2- 
d  data.  Proposition  2  will  use  bilinearity  to  quantify  the  effect 
of  misclassification. 

Proposition  2;  Suppose  vectors  v:]  and  vk  are  misclassi- 
fied,  i.e.  v3  is  matched  incorrectly  with  zk,  and  vk  is  matched 
incorrectly  with  Zj.  Then  D  changes  by 

AD  =  K(vj  -  vk,  wkzk  -  WjZj)  (10) 

Proof:  Most  terms  in  the  incorrect  summation,  D\  are  the 
same  as  the  correct  summation,  D.  Only  those  involving  Vj 
and  vk  differ.  Thus  from  (2),  D'  =  D  —  WjK(vj, Zj)  — 
wkK(vk,  zk)  +WjK(vk,  Zj)  +wkK(vj,  zk).  Since  K  is  linear 
in  each  of  its  arguments,  AD  =  D'  —  D  =  WjK(vk  — 
Vj,Zj)  +  wkK(yj  —  vk,zk).  Moving  the  weights  inside  K, 
AD  =  K(vk  -  Vj,WjZj  -  wkzk). 

□ 


Using  the  same  basic  steps  as  those  of  Theorem  1,  it 
can  be  shown  that 

dJ  ^  irr~  -  -w 

=  4  2_^  WiK  (p  -  aqi,  Zi)e 

i= 1 

Thus 

QL  QJ  w 

—  =  0  =  —  -  2Ae  =  WiK(p-  aqi,Zi)e-  2Ae 
oe  oe  r— ' 

i=i 

This  equation  can  be  significantly  simplified  by  use  of  the 
bilinearity  of  K 

n  n 

0  =  [4y ^WjK(p,Zi)  -  4a  WjK{q^  Zj)}e  -  2Ae 

i= 1  i= 1 

n 

=  4 K(p,  WiZi)e  —  4 aDe  —  2Xe 

=  4K(p,0)e-  4aDe-  2Xe  =  0 


Remarks:  Vectors  are  most  likely  to  be  misclassified  if  they 
are  close  together,  i.e.  Vj  —  vk  is  small  and  wkzk  —  WjZj  is 
small.  Prop.  2  shows  that  these  two  (typically  small)  vectors 
are  multiplied  together  to  change  D.  Hence  AD  is  also  small 
for  most  misclassifications.  D  totally  determines  the  solution, 
hence  the  solution  also  has  small  errors  when  close  vectors 
are  misclassified. 

The  next  proposition  shows  that,  if  the  data  points  are 
chosen  to  satisfy  a  mild  symmetry  condition,  then  the  non- 
linearly  coupled  estimation  of  pose  (combined  position  and 
orientation)  gives  way  to  an  analytic,  decoupled  solution. 

Proposition  3:  Given  zt,  qt  £  K3,  w.i  £  R+,  i  =  1 . . .  n, 
a  £  R+,  such  that 

n 

y  w^i = o  (ii) 

i= 1 

,  the  minimum  of 

n 

J  =  ywj\\Rzi  +  p-  aqi\\ 2  (12) 

i= 1 

over  g  =  ( R,p)  £  SE(3)  ( R  £  SO(3),  p  £  R3),  is  found  by 
first  calculating 

n 

D  =  yWiK(qi,Zi) 

i= 1 

where  K  is  the  bilinear,  symmetric  matrix  function  given  in 
(3).  The  eigenvector,  e of  D  corresponding  to  the  maximum 
eigenvalue  is  the  unit  quaternion  representation  of  the  optimal 
R.  The  optimal  p  is 

P-=  (13) 
Ei=i  wi 

Proof:  Multiplying  out  J  yields 

n 

J  =  +  2  (p  —  aqi)T Rzi  +  (p  —  acfc)T(p-  aqi)\ 

i= 1 

As  in  Theorem  1,  the  objective  will  be  rewritten  in  terms  of 
quaternions  and  a  Lagrangian  will  be  formed,  L  =  J  +  A(1  — 


where  D  is  calculated  from  (2)  by  replacing  u \  with  cfc.  A  zero 
argument  in  any  linear  function  returns  zero,  thus  /((■,  0)  = 
K{ 0,  •)  =  0.  Finally, 

—4  aDe  =  2Xe 


Thus  the  optimal  unit  quaternion,  e,  is  the  eigenvector  of  D 
corresponding  to  the  maximum  eigenvalue.  The  optimal  R 
can  be  found  directly  from  e  using  (5).  Once  R  is  found, 
the  translational  vector  p  can  be  easily  calculated: 


dL  _  dJ_ 
dp  dp 


n 

y;  Wi[2Rzi  +  2  p  —  2  aqi\ 

i=l 


Solving  for  p  yields  (13). 


□ 


Remarks:  Without  the  symmetry  condition  (11),  direct 
estimation  of  the  optimal  pose  requires  a  highly  coupled,  non¬ 
linear  numerical  optimization.  In  contrast,  the  new  solution  is 
very  simple  and  fast  to  calculate,  requiring  only  an  eigenvalue 
operation  on  a  4  x  4  matrix  for  R  and  a  summation  operation 
for  p.  The  condition  ulDi  =  0  is  very  mild  and  can 

often  be  realized  in  practice.  One  method  is  by  selecting  the 
points,  Zi,  so  they  satisfy  (11).  For  instance,  one  solution  is  to 
always  choose  the  s  in  symmetrical  pairs  so  both  z  and  -z 
are  members  of  z),  i  =  1 . . .  n,  and  both  z  and  —  z  have  the 
same  weighting.  This  can  often  be  accomplished  in  practical 
applications,  although  occlusions  may  limit  its  use  at  times. 

A  second,  fundamentally  separate  method  for  satisfying  (11) 
is  by  choosing  the  weights.  Letting  Z  =  [zj  zi . . .  zn\  and 
wT  =  [m;i  w2  ■  ■  ■  wn],  the  condition  ]U”=1  W; Zi  =  0  can 
also  be  written  as  Zw  =  0.  Given  Z,  w  must  be  chosen 
in  the  null  space  of  Z.  Generation  of  the  null  space  is 
very  straightforward.  However,  to  maintain  a  convex  objective 
function,  w  must  have  all  non-negative  elements.  This  can  also 
often  be  easily  accomplished,  see  [7]  for  further  details. 

Simultaneous  selection  of  both  the  data  points,  z,,  and 
the  weights,  Wj,  can  also  performed  to  increase  the  space 
satisfying  (11). 
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Also  note  that  when  calculating  the  optimal  pose,  the  data 
scaling  (a)  should  be  known,  as  it  is  needed  when  calculating 

V- 


each  data  vector,  up  to  a  sign.  That  choice  of  signs  minimizing 
the  objective  is  the  optimal  solution. 

□ 


III.  Optimal  Estimation  of  Pose  from  2-D  Data 

Affine  imaging  systems  [16]  are  a  limiting  case  of  perspec¬ 
tive  cameras  for  the  situation  wherein  the  depth  of  the  corre¬ 
spondence  points  is  much  larger  than  the  size  of  the  object. 
All  correspondence  points  are  then  scaled  by  approximately 
the  same  value,  and  the  data  loss  becomes  a  linear  loss  of  one 
dimension  (along  the  sensing  axis). 

Without  loss  of  generality,  assume  the  standard  camera 
model  with  camera  axis  along  the  “z”  dimension,  so  that  the 
missing  component  of  data  is  the  third  (or  “z”)  component. 
When  one  dimension  of  the  data  is  missing,  the  pose  estima¬ 
tion  problem  can  be  phrased  as: 

Given  Zi  G  R3,  di  G  R2,  Wi  G  R+,  i  =  1 . . .  n, 

P  =  [I2  0],  find  the  minimum  of 

n 

J  =  y^yWi\\P[Rzj  +p\-  adi\\ 2  (14) 

over  g  =  (R,p)  G  SE(3)  (R  G  SO(3),  p  G  R3),  and 
a  G  R+. 

Direct  minimization  of  (14)  involves  a  nonlinear  numerical 
optimization.  This  paper  will  derive  a  much  faster  method  via 
a  sequence  of  analytic  solutions. 


A.  Orientation  Estimation:  Scaling  Known 

First,  the  estimation  of  orientation  alone  will  be  treated.  As 
will  soon  be  explained,  orientation  estimation  from  2-d  data 
does  not  enjoy  the  invariance  to  scaling  found  in  Theorem  1, 
so  the  case  where  scaling  is  known  will  be  solved,  then  scaling 
will  be  estimated  separately. 

Proposition  4:  Given  i)  G  R3,  di  G  R2,  and  u>,;  G  R+, 
i  =  1 . . .  n,  P  =  [I2  0]  the  minimum  of 

n 

J2d  =  '52wi\\PRZi-di\\2  (15) 

i= 1 

over  R  G  SO(3),  is  found  by  first  calculating 


Next,  the  2"  3-d  orientation  problems  corresponding  to  all 
possible  i  =  1 . . .  n  ±  sign  choices  are  solved  using 
Theorem  1: 

n 


min  S^  WiWRzi  -  Vi\ 
RdSO(  3)~^ 


From  these  2n  solutions,  the  optimum  is  that  minimizing  Jid- 
Proof:  Let  vf  =  [d[  vZi]7  where  vZi  is  the  missing  com¬ 
ponent  of  Vi .  Since  rotation  of  a  vector  does  not  change  its 
length,  \\zi\\2  =  ||tTi||2  =  ||di||2  +  v2..  Therefore  vZi  = 

±  \J 1 1  Zi  1 1 2  —  1 1  d, 1 1 2 .  The  knowledge  of  the  vector’s  length  in 
one  frame  has  been  exploited  to  find  the  missing  component  of 


Remarks:  The  re-calculation  of  D  —  'G,  w,  K(v7,  z,  ) 

necessary  for  each  of  the  2n  solutions  of  the  3-d  optimal 
orientation  algorithm  (Theorem  1)  can  be  expedited  in  two 
ways.  First,  recall  that  Gray  code  is  a  binary  code  wherein  each 
successive  element  toggles  only  one  sign.  Thus,  by  searching 
through  the  signs  in  an  order  specified  by  Gray  code,  only  one 
sign  will  change  between  each  successive  solution.  Suppose 
D  has  already  been  calculated,  but  for  the  next  step  the  sign 
of  vZi  is  toggled: 


dj 

dj 

Vj+  = 

.  v*j  . 

-*■  *5-  = 

.  ~v*i  . 

The  new  D ,  D’ ,  is  D’  =  D  +  Wj[K(vj_,  Zj)  —  K{vj+,  Zj)\. 
Bilinearity  of  K  reduces  this  to  D’  =  D  +  WjK(Hj_  — 
vj+,Zj)  =  D  +  WjVZjI\(z,  Zj)  where  z1-  =  [0  0  1],  Inserting 
the  constant  z  into  (3)  yields 

’  -2^3,  0  zXj  Z2]  1 

A  D  =  D'-D  =  Wj  °  ~Zlj  (17) 

zh  z23  0  0 

.  *2,  -21,  0  0  _ 

where  zj’  =  [z\i  z^3  23 J  This  implies  that  each  successive 
solution  can  be  quickly  calculated-simply  insert  z:}  and  w:j 
into  (17),  to  get  the  new  D  =  D+AD.  The  optimal  quaternion 
is  then  the  eigenvector  of  D  with  maximal  eigenvalue. 

B.  Errors  Due  to  Scaling 

Although  estimation  of  orientation  from  3d  data  is  invariant 
to  scaling  (Theorem  1),  estimation  of  orientation  from  2d 
data  using  Prop.  4  is  not  invariant  to  scaling.  Thus,  before 
using  Prop.  4,  the  data  should  be  scaled.  However,  knowing 
the  scaling  is  tantamount  to  knowing  the  depth.  For  instance, 
under  the  perspective  transformation  common  to  most  optical 
systems,  image  measurements  are  scaled  by  the  depth.  It 
is,  of  course,  possible  to  use  another  separate  depth  sensor. 
However,  this  subsection  will  show  that  scaling  can  be  easily 
estimated  from  2d  data  and  target  geometry  for  any  orientation. 
Independence  from  a  particular  orientation  is  novel,  as  prior 
algorithms  have  required  coarse  knowledge  of  the  orientation 
to  get  coarse  scaling  estimates.  Estimating  scaling  is  closely 
related  to  estimating  depth.  See  [1]  for  a  more  complex,  yet 
linear  method  for  recovering  the  different  depths  of  several 
points,  when  the  affine  assumption  of  nearly  equal  depths  does 
not  hold. 

The  effects  of  scaling  estimation  errors  are  studied,  and  are 
found  to  depend  upon  the  geometry  of  the  data  vectors,  z„  as 
well  as  the  viewpoint.  Convex  methods  for  finding  the  worst 
case  viewpoint  and  estimation  error  for  a  given  set  of  data 
vectors  are  developed.  A  solution  to  the  problem  via  linear 
matrix  inequalities  is  derived  which  efficiently  finds  the  global 
optimum. 

Proposition  5:  If  the  true  scaling,  a,  and  estimated  scaling, 
ae,  differ  by  the  factor  (3  =  ae/a ,  then  the  angular  error 
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between  the  actual  measurement  vector,  u),  and  its  estimate, 

vei,  is 


9  =  acos 


PpI  +  \J(1  -PiX1  ~P2Pi) 


(18) 


where 


(19) 


is  the  percentage  of  the  measurement’s  norm  passing  through 
the  projection  onto  2d. 

Proof:  Let  Hi  =  \d[  vZi]T  as  in  Prop.  4.  Because  rotation 
matrices  preserve  lengths,  \\zi\\2  =  a2||'D)||2  =  a2(||di||2  + 
v2.).  Dividing  by  ||,?j||2  yields 


1  —  Pi  + 


2  2 
azvz. 


(20) 


Fig.  1.  The  angular  error  due  to  errors  in  estimating  the  scaling. 


Also, 


(21) 


Performing  the  same  calculation  using  an  estimated  scaling, 
ae,  rather  than  the  true  scaling,  a  gives  an  estimate  of  the 
vector  V{ 


Defining  the  multiplicative  scaling  error  as  /?  =  ae/a 
and  taking  dot  products  between  normalized  versions  of  (21) 
and  (22)  gives  the  cosine  of  the  angle  between  the  true  and 
estimated  measurement.  Simplification  yields  (18). 


□ 


C.  Estimation  of  Scaling  Using  the  LUB 


From  (20),  it  is  clear  that  pi  =  <  1.  Solving  for  a 

gives 

1 

°  -  mm 


This  equation  must  be  satisfied  for  all  of  the  data  points,  i  = 
1 . . .  n.  Consequently,  one  estimate  for  the  scaling  is  to  use 
the  least  upper  bound  (LUB),  i.e.  let 


ae  =  mm  — — 

i=1-n  ||d*| 


maxi=i...„  1 1 dj 1 1 /  |  | Zj | 


(23) 


The  maximum  percentage  of  norm  passing  through  the  2d 
sensor  is  then  from  (19) 


p  =  a  max 

z=l.  ..n 


From  (23)  and  (24), 


a  p 


(24) 


Inserting  (25)  into  (18)  yields  the  angular  estimation  error 
corresponding  to  the  least  upper  bound  estimation  of  a: 


9  =  acos 


(26) 


Figure  1  plots  9  vs.  pmax  (P)  and  P  ( Pi )■  Note  that,  if  p  =  1, 
then  8  =  0  (no  estimation  error).  In  this  case,  at  least  one 
Vi  lies  in  the  x-y  plane,  so  the  full  norm  passes  through  the 
compression  to  2d.  Note  also  that  9  decreases  as  p  increases, 
thus  designing  the  data  so  that  the  minimum  p  is  large  will 
reduce  errors. 


D.  Worst  Case  Maximum  p 

Since  Zi,  i  =  1 . . .  n  are  known  apriori’,  the  angles  between 
them  are  also.  Using  this  information,  it  is  possible  to  find 
the  worst  case  viewpoint,  i.e.  that  which  minimizes,  over  all 
possible  viewpoints,  p.  Let  a  be  the  worst  case  viewpoint.  For 
a  camera,  a  is  physically  the  unit  vector  pointing  along  the 
camera’s  optical  axis-commonly  this  is  made  the  “z”  vector  in 
the  camera  coordinate  system,  which  is  equivalently  the  third 
row  of  R.  Let  Hi  =  ^/||^||,  i.e.  normalized  data  vectors.  The 
following  theorem  finds  the  worst  case  viewpoint. 

Theorem  6:  The  worst  case  viewpoint,  a  £  M3,  i.e.  the  unit 
vector  such  that 

min  p  =  pmin  (27) 

aT  a— 1 

Can  be  found  by  solving  the  following  convex  problem  subject 
to  linear  matrix  inequality  constraints: 


max  t 
teR,  SeR3 


-rr -t 
a  rii 


-<t  — 
a  rii 


>0,  i  =  1, . . .  ,n 


(28) 


I3  a 
■t  ! 


a 


>  0 


Moreover,  the  minimum  maximum  percentage  norm  is 

Pmin  1  f 


(25) 


(29) 


IEEE  TRANSACTIONS  ON  ROBOTICS,  VOL.  X,  NO.  XX,  NOVEMBER  200X 


6 


The  worst  case  angular  estimation  errors  are  given  by 


acos 


Pmin 


'(1 -*?)(! -d, 

Prr 


pi 


(30) 


Proof:  Direct  solution  of  (27)  is  nonlinear  and  non-convex, 
therefore  the  optimization  is  slow  and  globality  of  the  mini¬ 
mum  is  not  guaranteed.  However,  a  complementary  problem 
can  be  equivalently  solved  in  a  convex  setting.  Since  data  along 
the  a  direction  is  lost  in  the  projection  to  2d,  bad  viewpoints 
have  a  large  component  of  the  data  along  a,  thus  d'  ri,  is 
large.  However,  if  even  one  data  vector  has  a  large  projection 
into  the  space  perpendicular  to  a,  then  that  vector  will  have  a 
large  percentage  of  its  norm  sensed,  so  p  will  in  turn  be  large. 
Consequently,  finding  the  worst  viewpoint  can  be  formulated 
as: 


max  min  \aT  nA  (31) 

aTa=  1  t=l,...,n 


This  can  be  equivalently  stated  as 


maxi 

a,t 

subject  to 

(aT  Pi)2  >  t2,  i=l, ...,  n  (32) 

aT  a  =  1 


The  nonlinear,  quadratic  constraints  can  be  replaced  with 
equivalent  linear  matrix  inequalities  (LMI’s)  by  using  the 
Schur  complement.  If 


X  = 


A  B 
Bt  C 


is  a  partitioned  matrix,  then  X  >  0  (X  positive  definite)  is 
equivalent  to  A  >  0  and  S  =  C  —  BTA~1B  >  0,  where  S  is 
the  Schur  complement  [2].  Thus  aTfli  >  0,  ( aTfii )2  —  i 2  >  0 
is  equivalent  to  X  >  0,  where  X  is  the  2x2  matrix 


->T  -> 
a  rii 


->T 
a  rii 


(33) 


Moreover,  the  objective  increases  as  the  norm  of  a  increases, 
thus  the  equality  constraint  aT a  =  1  can  be  replaced  with 
the  inequality  constraint  aT a  <  1.  This  nonlinear  inequality 
can  then  be  written  as  a  linear  matrix  inequality  by  using  the 
Schur  complement: 


h  a 

aT  1 


>0 


Thus  the  optimization  problem  (32)  can  be  expressed  as 
minimization  of  a  linear  objective  subject  to  linear  matrix 
inequalities  as  in  (28).  The  variable  t  is  the  component  of 
n  along  a,  where  n  represents  the  data  vector  with  smallest 
projection  along  a.  A  unit  vector  perpendicular  to  a,  a±,  can 
then  be  found  such  that  n  =  ta  +  prninaL.  Calculating  the 
norm  squared  of  both  sides  then  yields  1  =  t2  +  Pmin-  Re¬ 
arranging  yields  (29).  Finally,  (30)  is  obtained  by  substituting 
(29)  into  (26). 


□ 


Note  that  the  linear  matrix  inequality  solution  adds  an 
extra  condition,  aT Hi  >  0.  This  condition  cannot  be  satisfied 


A  good  choice  for  three  sensing  directions  (blue),  and  the  worst  viewpoint  (red,  dashed) 


Fig.  2.  Three  orthogonal  data  vectors  and  their  worst  case  viewpoint. 


unless  all  of  the  data  vectors  are  in  the  same  hemisphere. 
This  condition  poses  no  problem  since  the  magnitude  of  the 
projection  of  a  vector  is  the  same  as  the  magnitude  of  the 
projection  of  the  negated  vector.  Thus  n,;  not  in  the  upper 
hemisphere  are  negated,  without  changing  the  worst  viewpoint. 

E.  LM1  Solution  and  Data  Design 

The  global  optimum  of  the  convex  optimization  problem  in 
Theorem  6  can  be  quickly  found  with  a  number  of  software 
packages.  Figures  2  and  3  illustrate  some  solutions  obtained 
using  the  “mincx”  function  of  Matlab’s  Robust  Control  Tool¬ 
box.  Figure  2  illustrates  Z  =  I,  i.e.  three  data  vectors  are 
arranged  in  a  mutually  orthogonal  fashion.  The  worst  case 
viewpoint  minimizes  the  maximum  projection  orthogonal  to 
itself,  thus  in  this  case  a  =  [1  1  1]T /v/3  and  pmin  = 
0.82.  Substitution  into  (30)  quantifies  the  angular  errors.  This 
application  of  Theorem  6  illustrates  how  it  can  be  used  to 
assess  a  set  of  data  vectors,  Z. 

Theorem  6  also  immediately  suggests  methods  for  designing 
the  set  of  data  vectors,  Z,  so  they  inherently  provide  accurate 
estimates.  For  instance,  since  a  represents  the  worst  case 
viewpoint,  a  data  vector  added  in  the  space  perpendicular  to  a 
converts  this  worst  case  viewpoint  into  an  ideal  viewpoint,  thus 
eliminating  this  poor  viewpoint.  If  the  worst  case  viewpoint 
for  the  new,  augmented  set  of  data  vectors  is  found,  then  the 
data  vectors  can  again  be  augmented  with  a  vector  in  the  new 
tr1,  and  so  on.  Thus  a  good  set  of  n  data  vectors  can  be  found 
inductively  by  repeating  applying  Theorem  6  and  adding  data 
points  in  a  1  . 

Figure  3  illustrates  this  process  when  applied  to  the  data 
vectors  from  Fig.  2.  This  process  raises  pmin  from  pmvn  = 
0.82  to  pmin  =  0.95.  This  large  value  for  the  worst  case  p 
results  in  very  small  angular  input  errors  (see  Fig.  1  and  Eqn. 
(26). 
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A  good  choice  for  four  sensing  directions  (blue),  and  the  worst  viewpoint  (red  dashed) 


Fig.  3.  Four  data  vectors  and  their  worst  case  viewpoint. 


F.  Orientation  Estimation:  Scaling  Unknown 

Lemma  7  will  now  show  how  these  results  can  be  brought 
together  to  estimate  orientation  from  2-d  measurements  with 
unknown  scaling. 

Lemma  7:  Given  z.t  £  R3,  di  £  R2,  and  Wi  £  R+,  i  = 

1 . . .  n,  P  =  [I2  0]  the  minimizer  of 

n 

J  =  ^Wi\\PRzi- ad,\\2  (34) 

i= 1 

over  R  £  SO(3),  a  £  R+,  is  approximated  by  first  estimating 
a  by  a.  =  -  . - .  R  can  then  be  estimated 

by  replacing  all  measurement  vectors  di  with  scaled  versions 
di  1— >  aedi,  i  =  1 . . .  n,  and  applying  Prop.  4.  The  worst  case 
errors  can  then  be  found  from  Theorem  6. 

Proof:  Lemma  7  extends  Prop.  4  to  handle  differences  in 
scaling  between  the  data  vectors,  zz,  and  the  measurements, 
di.  The  LUB  estimate  of  scaling  given  by  (23)  will  contain 
error  unless  at  least  one  measured  vector  truly  does  lie  in  the 
sensor  (x-y)  plane.  Theorem  6  quantifies  the  worst  case  effects 
of  this  estimation  error. 


over  g=(R,p)£  SE(3)  (R  £  SO(3),  p  £  R3),  and  a  £  R+ 
can  be  estimated  by  forming  the  free  vectors  zzj  =  Zi  —  Zj, 
dij  =  cfi  —  qj,  i,j  =  1 . . .  n.  R  and  a  can  then  be  estimated 
using  Lemma  7  by  replacing  Zi  by  zZj  and  di  by  dZj .  The  first 
two  components  of  p  are  estimated  by 


Px 

.  Py 


=  pa  =  Pp  = 


a  E?=l  WiQi  ~  Ra  1  Wi*i 

En 

i= 1  wi 


(36) 


where  Ra  denotes  the  2x3  matrix  consisting  of  the  first  two 
rows  of  R.  When  the  measurements  arise  from  an  optical  sys¬ 
tem  with  focal  length  /,  and  pz  denotes  the  third  component 
of  p,  and  if  the  imaging  is  affine  ( 1 1  z)  1 1  «  pz  for  all  points 
i  =  1 . . .  n),  then 

Pz  ~  otf  (37) 

Proof:  Lemma  7  requires  free  direction  vectors  zj  and  di 
as  inputs.  However,  given  position  vectors  Zi,  free  direction 
vectors  can  be  constructed  by  subtracting  positions  to  find 
z^  =  Zi-Zj.  Their  corresponding  sensed  free  direction  vectors 
are  dij.  Weighting  of  these  free  vectors  can  be  performed 
in  accordance  with  their  known  error  characteristics,  or  their 
geometric  or  arithmetic  means  (w,j  =  ^ WiWj  or  Wij  = 
( Wi  +  Wj)/ 2).  The  data  and  measurements  are  then  ready 
for  application  of  Lemma  7  to  estimate  R  and  a.  With  these 
estimates  in  hand,  the  first  two  translational  elements  can  then 
be  estimated  by  the  unconstrained  least  squares  problem: 


n 

LS  =  min  V"'  w; | \Ra%i  +  P2  -  a<fi||2 

Pa  ' 

1=1 


Setting  =  0  yields  (36).  To  find  the  depth,  pz,  consider 
a  point  at  the  origin  of  the  data  frame.  Its  representation  in 
the  sensor  frame  is  p.  Orienting  the  optical  system  along  the  z 
axis,  the  perspective  transformation  caused  by  a  focusing  lens 
gives  the  image  plane  position,  q: 


Pz 


But  aq  =  pa ,  thus  pz  =  af.  If  ||z,;||  <C  pz  for  all  points  i  = 
l...n  (affine  imaging),  then  the  perspective  transformation 
scales  all  image  points  by  very  close  to  the  same  factor,  thus 
one  a  suffices  for  the  entire  set  of  data,  and  it  is  proportional 
to  depth. 


□ 


□ 

G.  Pose  Estimation  from  2-d  Measurements 

Prop.  3  derives  a  decoupled  method  of  calculating  the  pose 
from  3-d  measurements.  Unfortunately,  the  effects  of  scaling 
preclude  a  similar  result  from  2-d  measurements.  This  section 
develops  techniques  of  extending  the  2-d  orientation  methods 
developed  herein  to  estimate  both  orientation  and  position. 

Proposition  8:  Given  data  points  z.;  £  R3  and  correspond¬ 
ing  sensed  points  qi  £  R2,  along  with  weightings  Wi  £  R+, 
*  =  1 . . .  n,  P  =  [I2  0] ,  the  minimizer  of 

n 

J  =  'Y^wl\\P[Rzi+p\- aqi\\2  (35) 

i=l 


IV.  Simulation  Results 

Figures  4-5  compare  the  performance  of  the  new  orientation 
estimates  found  using  Theorem  4  to  estimates  obtained  from 
the  standard  approach-use  of  Theorem  1,  with  the  unknown 
“z”  component  of  vt  assumed  to  be  zero.  Figure  4  plots  the 
measured  image  plane  direction  vectors,  Vj,  (solid).  R  and  a 
are  then  estimated  by  the  techniques  of  both  Theorem  1  (with 
vZi  =  0)  and  Theorem  4.  From  the  data  vectors  Z,  the  image 
plane  direction  vectors  are  then  reconstructed:  vei  =  Rfi/a. 
R  and  Z  are  intentionally  chosen  to  produce  large  enough 
errors  to  be  visible.  Fig.  4  illustrates  that  the  new  method 
does  produce  better  reconstructions  of  the  image  directions. 
Indeed,  the  angle  required  to  rotate  the  true  R  to  its  estimate 
is  33°  for  the  standard  approach  vs.  only  11°  using  the  new 
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Fig.  4.  Three  image  plane  directions  (solid),  their  reconstruction  using  the 
standard  approach  (dotted),  and  their  reconstruction  using  the  new  method 
(dashed). 


Fig.  5.  Five  image  plane  directions  (solid),  their  reconstruction  using  the 
standard  approach  (dotted),  and  their  reconstruction  using  the  new  method 
(dashed). 


techniques  developed  in  Theorem  4.  Fig.  5  displays  the  same 
results  when  n  =  5,  rather  than  n  =  3,  measurements  are 
taken.  In  this  case,  the  angle  required  to  rotate  the  true  R 
to  its  estimate  is  21°  for  the  standard  approach  vs.  only  1.4° 
using  the  new  method.  These  results  are  typical  comparisons. 

Fig.  6  compares  three  pose  estimation  techniques.  From 
four  given  image  points  (denoted  by  +),  the  pose  (f?,p)  and 
scaling  a  are  estimated,  then  used  to  attempt  a  reconstruction 
of  the  given  image  points.  This  gives  a  visual  indication  of  the 
pose  estimation's  accuracy,  which  will  also  be  complemented 
with  the  actual  estimation  errors.  The  actual  rotation  matrix 
is  chosen  so  it  has  the  worst  case  viewpoint  calculated  via 
Theorem  6.  The  position  vector  is  p  =  [0.369  0.179  105]T. 
Its  “z”  component  is  100km  because  our  current  application 
involves  ground  based  images  of  satellites. 

For  comparison  purposes,  the  pose  is  first  estimated  using 


4* 


5 

x  10'6 


Fig.  6.  Four  image  plane  points  (+),  their  reconstruction  using  the  standard 
approach  (*),  their  reconstruction  using  the  new  method  (0),  and  their 
reconstruction  using  full  3-d  sensed  data  (pentagram). 


3-d  measurements.  This  method  requires  another  sensor  such 
as  a  depth  sensor  or  a  stereo  camera,  and  estimation  of  pose 
from  such  data  sets  is  a  mature  science.  One  technique  is  to 
find  free  vectors  by  subtracting  the  points  in  the  same  manner 
used  by  Prop.  8.  Theorem  1  can  then  use  these  3-d  free  vectors 
to  estimate  R  and  a.  Finally,  the  optimal  p  can  be  found 
using  (13).  Since  this  method  has  access  to  the  full  3-d  data, 
its  reconstructed  image  points  (pentagrams)  closely  match 
the  given  image  points  (+).  A  simple  application  of  these 
same  ideas  using  2-d  data  is  to  assume  that  all  measurement 
points  qi  have  “z”  components  equalling  zero,  then  apply  the 
above  3-d  algorithm  after  augmenting  each  with  a  zero 
“z”  component.  This  is  termed  the  standard  approach,  and 
reconstructions  based  on  it  are  depicted  by  (*)  in  Fig.  6.  The 
standard  approach  performs  erratically,  usually  providing  very 
poor  pose  estimates.  Reconstructed  image  points  found  using 
the  new  technique  derived  in  Lemma  8  are  illustrated  by  (0). 
Like  the  full  3-d  algorithms,  they  closely  match  the  given 
image  points,  but  unlike  the  full  3-d  algorithm,  the  estimates 
can  be  obtained  from  a  single  monocular  image. 

The  table  below  documents  the  angle  required  to  rotate  the 
estimated  R  to  the  true  If  eg.  Letting  the  translational  error  be 
ep  =  p—pestimated ,  the  percentage  of  normed  translation  error 
for  each  case  is  also  available,  ||ep||/||p||.  While  the  algorithm 
that  has  access  to  the  full  3-d  measurements  performs  best,  its 
accuracy  is  rivaled  by  the  new  algorithm,  which  uses  only 
2-d  sensor  readings.  The  standard  approach  has  very  erratic 
behavior.  It  this  case,  its  estimates  are  poor,  although  other 
simulations  exhibit  far  better  and  far  worse  accuracy. 


Full  3-d 

Standard  2-d 

New  2-d 

\\eP\\/\\p\\ 

8  x  10"6 

0.44 

0.07 

eg 

0.27° 

30° 

1.4° 

The  new  method  has  been  developed,  in  part,  to  aid  in  the 
automatic  inspection  of  satellites.  From  a  noisy,  diffraction 
limited  image  of  a  known  satellite  in  an  unknown  pose,  the 
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Fig.  7.  CAD  model  of  the  imaged  satellite,  with  O’s  denoting  the  point 
correspondences. 


Sequence  of  Satellite  Images. 


0  50  100  150  200  250  300 


Fig.  8.  As  the  satellite  sweeps  across  the  sky,  several  images  are  captured. 

pose  will  be  estimated.  The  CAD  model  can  then  be  placed 
in  the  same  pose  and  projected  onto  two  dimensions  to  create 
a  synthetic  prediction  of  what  the  satellite’s  image  should 
look  like.  Subtraction  of  the  actual  image  from  the  synthetic 
image  produces  a  signal  useful  for  automatically  checking  any 
satellite  anomalies. 

Fig.  7  depicts  a  hypothetical  satellite.  The  far  corners  of  the 
solar  arrays  are  chosen  as  the  data  points  (zi,  i  =  1, ...  ,4, 
depicted  as  O’s),  as  they  produce  an  easily  identifiable  signa¬ 
ture.  As  the  satellite  moves,  several  images  are  captured  (Fig. 
8).  The  pose  will  be  estimated  and  the  satellite  inspected  for 
images  1  and  4  (1  is  the  leftmost). 

Because  satellites  orbit  at  significant  distances  from  the 
Earth,  the  actual  image  is  degraded  appreciably  by  diffraction 
effects  (see  Fig.  9).  Using  a  binary  thresholding  technique,  it 
is  enhanced,  and  the  solar  array  corners  in  the  image  (g))  are 
found  (Fig.  10). 

Despite  the  effects  of  diffraction,  thresholding,  and  pixel 
round-off,  the  pose  estimate  is  quite  accurate,  producing  an 
angular  estimation  error  of  only  eg  =  5.3°.  A  thresholded  ver¬ 
sion  of  the  predicted  image  is  subtracted  from  the  thresholded 
measured  image  (Fig.  10)  to  produce  the  inspection  image 
(Fig.  11).  Although  it  suffices  for  our  present  purposes,  an 


Diffracted  Image 
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Fig.  9.  The  first  raw,  diffracted  image. 


Satellite  Image  and  its  Estimated  Vertices  (o) 


0  50  100  150  200  250  300 


Fig.  10.  The  first  image  with  the  sensed  points,  cfo,  denoted  by  (0). 

improved  match  can  certainly  be  obtained  from  this  subtracted 
image  by  employing,  for  example,  the  methods  in  [5]  to  fine 
tune  the  estimate. 

Automatically  determining  the  point  correspondences  g)  can 
be  difficult.  These  simulations  first  find  a  point  inside  the 
satellite  by  a  very  coarse  template  matching  strategy.  Then, 
a  statistical  snake  [13]  grown  from  that  initial  point  robustly 
identifies  the  perimeter.  Finally,  vertices  along  that  perimeter 
are  extracted  (see  [11]  for  further  details).  Figures  12  and  13 
illustrate  the  results  on  image  4.  In  this  case,  eg  =  25°. 

V.  Conclusions 

For  the  case  of  affine  imaging,  this  paper  replaces  nonlin¬ 
ear  numerical  iterations  or  extremely  high  dimension  linear 
solutions  with  solving  the  standard  3d-3d  optimal  orientation 
problem  2”  times,  where  n  is  the  number  of  data  points.  The 
2n  successive  absolute  orientation  calculations  are  speeded 
through  use  of  Gray  code,  and  have  the  dual  advantages  of 
speed  and  predictable  execution  time.  Angular  errors  caused 
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Matched  Image  minus  Measured — gray  means  match  is  good 


0  50  100  150  200  250  300 


Fig.  1 1 .  Combining  pose  estimates  with  the  CAD  model,  a  predicted  image 
is  calculated  and  subtracted  from  the  measured  image. 


Satellite  Image  and  its  Estimated  Vertices  (o) 


0  50  100  150  200  250  300 


Fig.  12.  The  fourth  image,  with  its  points  qi  denoted  by  (0).  The  outside 
perimeter  is  identified  by  statistical  snakes. 


Matched  Image  minus  Measured — gray  means  match  is  good 


0  50  100  150  200  250  300 


by  scaling  imperfections  are  quantified,  and  a  least  upper 
bound  estimate  of  the  scaling  is  proposed.  It  is  shown  that  the 
worst  case  viewpoints  depend  only  on  the  data  points  chosen, 
and  a  new  convex  linear  matrix  inequality  optimization  is 
derived  for  determining  the  worst  viewpoint.  This  new  analysis 
tool  is  useful  for  evaluating  a  particular  set  of  data  and  suggests 
methods  of  designing  the  data  for  high  performance.  Simu¬ 
lation  results  for  a  satellite  inspection  problem  demonstrate 
the  viability  of  the  technique  and  its  superiority  over  standard 
methods. 
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Fig.  13.  Combining  pose  estimates  with  the  CAD  model,  a  predicted  image 
is  calculated  and  subtracted  from  the  fourth  measured  image. 


